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Abstract — This paper presents methods for the generation 
of optimal reaching and balancing movements in humanoid 
robots. Arm reaching movements are parametrized as a linear 
combination of basis elements, which are extracted via a principal 
component analysis of human motion capture data. Human- 
like reaching movements are then generated by determining 
the minimum torque motion from this parametrized class of 
arm movements. The optimal balancing problem, in which the 
goal is to execute a desired movement without losing balance, is 
framed as second-order cone programming (SOCP) problem; it 
is also shown that many existing optimization-based balancing 
strategies reduce to special cases of this more general formulation. 
Simulation results with a humanoid model are presented for both 
reaching and balancing movements. 



I. Introduction 

THIS paper presents methods for generating optimal reach- 
ing and balancing movements in humanoid robots. Past 
approaches to dynamics-based optimal robot movement gen- 
eration, e.g., [1], [7], [4], have been computationally pro- 
hibitively expensive for online generation. To address these 
computational difficulties, attempts to emulate the human 
capability of storing and reusing movement modules have 
also been made in different contexts throughout the robotics 
and human motor control literature. The idea of representing 
and generating human arm motions as a linear superposi- 
tion of principal components was first investigated in [13]. 
Similar approaches using human motion data have also been 
investigated for deriving action and behavior primitives for 
humanoid robots [3], [9], although these approaches are ex- 
clusively kinematics-based. It should also be noted that the 
optimal movements generated offline using any of the previous 
dynamics-based optimization schemes can in principle be used 
as primitives for schemes such as the one presented in this 
paper. 

To address the optimal reaching problem, various arm 
reaching movements are parametrized as a linear combination 
of basis elements, which are extracted via a principal com- 
ponent analysis of human motion capture data. Human-like 
reaching movements are then generated by determining the 
minimum torque motion from this parametrized class of arm 
movements. The proposed method partially overcomes the two 
main disadvantages of dynamics-based motion optimization 
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algorithms such as [1]: the computational inefficiency, and the 
lack of modularity and reusability. 

The second component of this paper addresses the prob- 
lem of active balancing of legged robots, also referred to 
variously in the literature as "whole-body cooperative bal- 
ancing," "whole-body stabilization," or "posture control." The 
balancing problem involves the correction of a robot's motion, 
usually in real-time, to prevent the robot from tipping over 
while being supported by one or more legs. An input reference 
trajectory, not necessarily stable or even dynamically feasible 
{e.g., torque limits may be exceeded), is typically assumed 
given. The objective is to correct the actual motion, in real- 
time, so as to maintain balance in the presence of external 
disturbances while closely following the reference trajectory. 

Almost all of the existing active balancing methods involve 
the zero-moment point (ZMP) [15], [16], and in most cases 
the center of mass (COM) as well. Among the optimization- 
based balancing strategies — the focus of this paper — Kagami 
et al. [5] develop an active balancing algorithm for flat- 
footed biped robots, in which a least squares joint tracking 
error is minimized while maintaining the projected COM 
at a predefined fixed point, and confining the ZMP to the 
support region interior. Sugihara and Nakamura [14] also 
propose a quadratic programming-based method that takes into 
account constraints on the COM and ZMP, but assumes as 
input an already balanced trajectory that is subject to small, 
short-term disturbances. Kudoh et al. [6] present a quadratic 
programming strategy that minimizes joint accelerations while 
confining the ZMP to a prescribed region, and taking into 
account constraints on the COM acceleration, and structural 
symmetry of the legs {e.g.. both feet of the biped are assumed 
in contact with the ground at all times). Yamane and Nakamura 
[17] also present a combined optimization and PD feedback 
control approach to correcting general motions that minimizes 
the error between the reference and actual joint accelerations. 

As evident from the existing literature, from an optimization 
perspective active balancing can be formulated in numerous 
ways and under a variety of assumptions, e.g., minimizing a 
joint space tracking error while fixing the projected COM and 
confining the ZMP to the support polygon, minimizing the 
deviation of the ZMP from the support polygon center while 
maintaining a joint tracking error tolerance, compensating for 
projected COM tracking errors by a PD type feedback control 
of the projected COM acceleration, etc. 

One of our contributions is a general convex optimization 
based framework for the active balancing of legged robots that 
includes as special cases many of the previous approaches 
proposed in the literature. We identify a family of active 
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balancing algorithms that can be formulated as second-order 
cone programming (SOCP) problems. As is well-known, 
convex optimization problems admit global solutions, and 
interior point algorithms for SOCP have been reported to 
typically converge in 30 or so iterations regardless of the 
problem dimension (see, e.g., [2], [8]). 

The paper is organized as follows. In Section II we describe 
the optimal reaching algorithm, including case studies of 
several arm reaching movements using a humanoid model. 
Section III describes our SOCP balancing algorithm, and also 
includes simulation studies involving a biped humanoid model. 

II. Optimal Arm Reaching 

The arm reaching movement primitives are stored as a 
family of basis functions, with each basis function representing 
a joint trajectory. Just as an arbitrary continuous function 
can be represented a linear combination of trigonometric 
functions using Fourier series, our premise is that arbitrary 
joint trajectories should be represented as a linear combination 
of an appropriate set of basis functions. Rather than use, 
e.g., Fourier series or other standard basis functions from 
approximation theory, employing a customized set of basis 
functions for movement generation is both more economical 
while leading to more physically desirable motions. 

For illustration purposes, suppose arm motions are classified 
into, e.g., reaching, swinging, lifting light objects, and lifting 
heavy objects; each of these classifications is then regarded 
as a movement primitive. For each movement primitive we 
extract a set of k basis functions (j)i{t) ^ . . . ^ (j)k (t) for each 
joint. Clearly the (j)i{t) do not form a basis in the strict 
mathematical sense; rather, they are intended as a rich set 
from which a large number of joint trajectories can be closely 
approximated. In our approach each movement primitive is 
stored as a family of basis functions {(/>i(t), . . . , (/)^(t)} for 
each joint. An arbitrary trajectory for that particular joint is 
then represented as the linear combination 



<i){t) = j2ci<piit) 



(1) 



i=l 



with the Ci scalar constants to be determined. In the next 
section we describe how to extract the basis functions (/>i(t) 
from a video database. 

A. Extraction of Movement Primitives: PCA 

Like [3], the movement primitives are extracted via a 
principal component analysis of motion capture data. For 
example, to construct the basis functions corresponding to 
an arm reaching primitive, we first obtain motion capture 
data of a human subject repeatedly performing such motions; 
the motions should be similar enough to all be classified as 
reaching motions, yet diverse enough to be able to represent 
a range of different reaching motions, e.g., reaching upward, 
downward, to the sides, etc. 

Once the input motion data has been organized into different 
types of reaching movements, we then perform a principal 
component analysis for each cluster of trajectories; the most 



dominant principal components are then used as a set of 
interpolating basis elements representative of all trajectories 
in that cluster. Principal component analysis (PCA) is a well- 
known and widely used method for statistical analysis, and the 
detailed procedure for extracting principal components from 
our arm motion data is described in [11]. 

B. Motion Optimization 

The equations of motion for our systems, which are modeled 
as a set of coupled rigid bodies, are of the form 



M{q)q^C{qA)q + N{q,q)=T 



(2) 



where M[q) e 3?^><^ is the mass matrix, C{q,q) G 3?^><^ 
is the Coriolis matrix, and N{q^q) G 3?^ includes gravity 
and other forces. We will be interested in minimizing cost 
functionals of the form 

J{T) = ^q,q,tf)+ / L{q,q,T,t)dt (3) 

subject to Equation (2) and appropriate boundary conditions, 
where the terminal criterion function $ penalizes deviations 
from the desired final condition. We find a local solution to the 
optimal control problem by assuming that the joint coordinates 
q{t) in (2) are parameterized as a linear combination of the 
principal components, e.g.. 



q{t) = ci • PCi{t) + . . . + C4 • PC^it) + C5. 



(4) 



The original optimal control then reduces to a parameter 
optimization problem of the form 



Min 

c 



ftf 
J{c) = $(c,t/)+ / L{c,t)dt 
Jo 



(5) 



subject to various limits on the joint torques, positions, and 
velocities. Using more principal components allows for more 
varied joint trajectories, but also clearly increases the compu- 
tational burden of the optimization. 

C. Case Study: Human Arm Movements 

To demonstrate the computational feasibility of our ap- 
proach, we compare the arm movements generated using our 
approach with that obtained through straightforward dynamics- 
based optimization. Gains in computational efficiency are 
empirically assessed, and the degree to which our proposed 
method generates more natural arm motions is qualitatively 
evaluated. 

A principal component analysis of typical human arm 
movements is first performed. The human arm is kinematically 
modeled as a four dof serial chain, consisting of a three dof 
shoulder joint (represented as a serial 3 -re volute joint with 
intersecting orthogonal axes) and a one dof elbow joint. The 
movements of a human subject repeatedly performing various 
arm movements (i.e., raising, reaching, pulling, twisting, and 
swinging) are then captured and, following data cleansing 
and smoothing, and the solution of the inverse kinematics 
from the motion capture data (given by marker position data) 
stored in the form of joint trajectories. To ensure that the 
same number of sample data are stored for each motion, all 
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joint trajectories are time- scaled to the same time interval; a 
principal component analysis of this data is then performed. 
For our data, four principal components are sufficient to 
represent over 99% of the captured arm movements. 

Each principal component and its first and second deriva- 
tives are stored in table form; this will be useful for imple- 
menting fast lookup in the dynamics-based optimization pro- 
cedure. Fig. 1 shows all the principal component trajectories 
of the shoulder and elbow joints, and their first and second 
derivatives. These trajectories are obtained by cubic spline 
interpolation of the discrete principal component table values. 




Time(sec.) 



Fig. 1. Principal components of each joint. 



Time (sec.) 



TABLE I 

Optimization using principal components 



Number of variables 


12 


16 


20 


24 


Number of PCs 


2x4dof 


3x4dof 


4x4dof 


5x4dof 


Number of iterations 


8 


21 


43 


59 


Cost function value 


428.6 


248.1 


229.5 


212.1 


Time (sec.) 


25.1 


78.9 


195.6 


328.3 



TABLE II 

Optimization using B-splines 



Number of variables 


12 


16 


20 


24 


Number of control points 


7x4dof 


8x4dof 


9x4dof 


10x4dof 


Number of iterations 


17 


24 


40 


38 


Cost function value 


321.5 


287.8 


273.2 


244.3 


Time (sec.) 


227.0 


397.0 


795.4 


905.9 



The torque minimization was performed using Matlab ver- 
sion 6.5, running on a Pentium 4 (2.4 GHz) personal computer. 
As evident from the previous tables, our results indicate 
clear advantages in computational efficiency for the PCA- 
based approach. Note that the final objective function values 
achieved for the two approaches are similar. However, the final 
optimized motions for the two cases show notable differences; 
irrespective of the number of control points used, motions 
generated using the B- spline approach in general appear less 
natural than those of the PCA-based approach. In the case 
of arm raising motions (the arm is initially hanging down 
at rest, and raised vertically above the shoulder), for the B- 
spline motion, the elbow is bent at a sharper angle to utilize 
the kinematic singularity (thereby reducing the total applied 
torque), and shows fast movement in the latter phase of the 
reaching movement. Also, because the principal components 
are extracted from observations of human arm movement, it 
comes as no surprise that PCA-based motions appear much 
closer in shape and profile to human arm movements than 
their B-spline counterparts. 

in. Optimal Balancing 

We now describe our SOCP algorithm for optimal balanc- 
ing; further algorithmic details can be found in [12]. Given a 
biped humanoid model, we denote the fixed reference frame 
by {o}, and attach a frame {r} to the point at which the 
floor exerts a reaction force on the sole of the foot. The 
shape of each sole is assumed to be rectangular. The wrench 
T G se* (3) denotes the reaction moment and force expressed 
in the {r} frame. Let To denote the wrench T expressed in 
the fixed frame, i.e.. To — Ad^JT. In anticipation of our later 
optimization formulation, we define the vector q — (Vq,^). 
where Vb ^ se(3) is the twist velocity of the pelvis and 
Q G 3?^ is the vector of joint variables. 

1) Constraints on the ZMP: The zero-moment point can be 
computed from the following set of equations: 



Pzmp 

A, 



^zmp 



To 



^zvnpJ~ o 



(6) 



^zmp 






-1 


1 












J3x3 



^[0 0000 1]g3? 



e^ 



1x6 



3x6 



Here the scalar (3 is set to either +1 or —1 so as to satisfy 

^zmp-^o ^ U. 

The requirement that the computed ZMP lie in the interior 
of the support polygon S {i.e., the convex hull defined by 
all the points of contact between the floor and robot) can be 
expressed as a linear inequality of the form 



A^p.^p + ^^ < 0, 



(7) 



for some matrix A'^ and vector b'^ . From Equation (6) the 
above assumes the more detailed form 



[A Azmp H" b bzmp) J~o ^ U. 



(8) 
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In general, the above constraint will lead to a nonconvex 
optmization problem. One common way to remedy this situ- 
ation is to provide a desired ZMP trajectory pf^^ as input, 
and to further require that, in addition to (7), the actual ZMP 
remain within some prescribed distance Czmp of the desired 
ZMP: 



Wpz 



' Pzmp I 



<e. 



(9) 



Again taking into account (6), the above can be more specified 
in more detail as 



[A, 



■P. 



zmp"zinp 



)^o\ 



S ^zmp^zmp-^o 



(10) 



where again bzmp^o > 0. It is also not essential to provide 
piynp as input; one possibility is to calculate the ZMP from 
the input reference motion, and in the event that it lies outside 
the support polygon, to project it to lie slightly inside the 
support polygon (or, even more simply, to take the center of 
the support polygon as the reference ZMP trajectory). 

2) Constraints on the COM: As is well-known, in the event 
that the support polygon is fixed, once the projected center of 
mass ventures outside the support polygon, gravitational roll 
and pitch moments that cause tipping are inevitably generated. 
Active balancing thus requires simultaneous consideration of 
both ZMP and COM constraints, as well as friction constraints. 
In [5], COM constraints are expressed as an equality con- 
straint, in which the projected COM is fixed to a predetermined 
point within the support polygon. While this has the advantage 
of a simpler optimization formulation, it has the obvious 
disadvantage of excessively restricting the solution space when 
compared to, e.g., simply requiring the projected center of 
mass to lie within the support polygon. 

Since the projected center of mass is completely determined 
by 0, in our optimization framework we seek a q = {Vo,0) 
that ensures that, at the next timestep, the projected center 
of mass also remains within the support polygon. Denote by 
Pcom{t) the center of mass at the current time t. From the 
finite-difference approximation 

Pcom{t + At) ^Pcom{t) + Pcom{t)At + -pcom{t)Af, (H) 



the center of mass at the next timestep, denoted p 
be approximated as 



corajnex 



t, can 



Pcomjnext 



= Ar 



tq + bc 



(12) 



for appropriate Acom.next and bcom.next- The requirement that 
the projected COM lie inside the support polygon S can now 
be expressed in linear inequality form: 



Acom^Pc 



+ ^com < 0, 



(13) 



where P denotes the projection matrix to the ground, and 
A^Q^ and h^o^ are appropriately obtained. As a matter of 
practical implementation, to compensate for errors in the finite- 
difference approximation it is beneficial to use a slightly 
shrunken version of the support polygon S. A similar strategy 
is suggested in [14], albeit in a somewhat different context. 

In [6] an active balancing scheme is proposed in which the 
trajectory of the desired projected COM, denoted Ppcomi^)^ 
is assumed given as input; the acceleration of the projected 



COM is then expressed in proportional-derivative feedback 
control form, in which the proportional and differential errors 
between the desired and actual projected COM are multiplied 
by suitably small gains kp and kd, and summed to obtain 
the compensation term. Such schemes can also be included 
into our general convex optimization framework. Denote the 
acceleration of the projected center of mass by 

where rrii and a^ respectively denote the mass and acceleration 
of link i, AddcomiO) e 3?3><^, and bddcom{Vo,0,0) e ^\ 
Ppcom is then assumed to be of the form 

Ppcom — ~l^d [Ppcom ~ Ppcom) ~ ^P [Ppcom ~ Ppcom) ' v^^/ 

Assuming the Ppcom trajectory remains inside the support 
polygon. Equation (15) ensures that the robot maintains a 
statically balanced posture. 

If Ppcom i^ supplied as an input, then Equations (14) and 
(15) lead to an equality constraint of the form 

^eq,staticQ — ^eq, static (tOj 

Alternatively, one can make the assumption that Ppcom = 0^ so 
that Ppcom i^ ^ fixed point inside the support polygon. In this 
case one can solve Equations (14) and (15) to obtain p^^^ as a 
function of q. Confining Pcom to the support polygon interior 
leads to an inequality constraint of the form 



J^ r)" 4- h'^ < 

^^comypcom ^^ ^com — ^' 



(17) 



Equations (15) and (17) can be expressed collectively as 



^static 



q< bs 



(18) 



We remark that other linear equalities and inequalities can be 
obtained via alternative methods, e.g., [5]. 

3) Friction Constraints: To prevent the foot from slipping, 
both the force tangent to the ground, and the moment normal 
to the ground, need to be considered. In the case of a single 
foot contacting the floor, the basic frictional constraints can 
be expressed as 

WPxyJ'oW < I^F, (19) 

WPmzJ'o - Pzmp X P^yJ'oW < /^rF2. (20) 

Fi+F2< PzTo (21) 

where Pxy, Pmz^ and P^ are the projection matrices required 
for obtaining the elements of force in the xy plane, the moment 
normal to the floor, and the force along the z axis, respectively. 
fji is the frictional coefficient in the plane; fir is the torsional 
frictional coefficient normal to the floor; Fi and F2 are the 
forces required to prevent the tangential force and normal 
moment from causing slippage. For the stationary foot, only 
the sum of these undetermined forces needs to be less than 

Note that the ZMP is not known a priori', we can only 
assume \\pzmp - pimpW < ^zmp- Since p^mp depends on To. 
(20) would not lead to a cone constraint, and the resulting 
optimization would not be convex. To circumvent this we set 
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Pzmp = pimp + ^Pzmp\ then from the metric property, we can 
write the left-hand side of (20) as 

\\J^^^mz-^o Pzmp ^ ^xy-'o ^Pzmp ^ -^xy-^oW 
< WPmz^Fo-pimp X Pxy^oW + W^Pzmp X P.yToW (22) 

From (9) and (19), we can rewrite 

\\J^^^mz-^o Pzmp X i^^xy-'oW 
S W-t'^m.z^o Pzm.p X t^xy^ o\\ \ ^zmpl^^l- \^-^) 

Noting that Equation (20) is always satisfied if 

WPmzTo - pimp X Pxy^FoW + C.mpl^Fi < /i^Fs, (24) 

the resulting inequality is obtained as 

WPmz^o - pimp X Pxy^oW < /^ri^2 " e^mp/^Fl- (25) 

In the course of simplifying the rotational friction constraints 
via Equation (22), we have in effect imposed additional 
constraints beyond those physically required. In the event 
that Czmp is set to zero, the additional constraint disappears, 
while for larger values of Czmp the effects of this additional 
constraint increase. 

In summary, given a desired ZMP trajectory pf^^ and 
threshold e^^p, and assuming multiple contacts, from the 
above simplification the frictional constraints can now be 
expressed explicitly as follows: 

||P,^Ad^^.^,-||<A,- Vj (26) 

WiP^z - [pimp] Pxy):Fo\\ <V- e.mp J] A,- (27) 



A, 



— + — < Pz:Fo 



(28) 



where we use the notation pi x p2 = [pi]p2 for any pi,p2 ^ 
3?^, j is the foot index for multiple contacts, and Xj, v are 
undetermined variables satisfying (26), (27), and (28) (note 
that it is unnecessary to obtain \j and v explicitly). 

4) Contact Kinematic Constraints: The constraint that the 
supporting feet remain stationary e.g., lifting or translating of 
any contacting feet is not permitted, in both static and dynamic 
environments, leads to an additional set of equality constraints. 
Denoting by Vj G se(3) the body twist velocity for foot j, one 
then obtains the relation 



Vj =Ayd + b, 



(29) 



where Ay.{0) e ^^""^ and by.{Vo,0,0) e ^\ This twist 
velocity is set to zero in the case of a static floor, or can be 
made to coincide with the velocity of the floor in the case of a 
dynamic environment. By stacking all such equations for each 
contacting foot, a linear equality constraint in q of the form 



^V.^^.+ .^ ~ ^consts b 

*' con sts 



*^const 



(30) 



is obtained. See, e.g., [10] for a systematic derivation of the 



A. Second-Order Cone Programming 

A second-order cone programming (SOCP) problem can be 
expressed in the following form: 



fx 



(31) 



subject to \\AiX + hi\\ < cjx -\- di i = 1, . . . , iV,(32) 



Fx = g, 



(33) 



where x G 3?^ is the optimization vector, F G 3?^^^. 

/ e 3?^, Ai e 3?(^^-i)><^, 6i e 3?^^-\ci e 3?^, ^i e 3? 

are given constants. The norm is the standard Euclidean norm, 
i.e., \\u\\ = [u^u) . Inequality constraints of the above form 
are called second-order cone constraints, since it is the same 
as requiring the affine function {AiX + bi,cjx + di) to lie 
in the second-order cone in 3?^\ The above optimization is 
a nonlinear convex problem that includes linear and quadratic 
programs; in fact, if each c^ = the SOCP problem reduces 
to a quadratically constrained quadratic programming problem. 
Details of the SOCP problem, as well as the various interior 
point algorithms for their solution, can be found in [2]. 

B. General SOCP Formulation for Active Balancing 

For the basic case of minimizing acceleration tracking 
errors, in which both the COM and ZMP constraints are 
expressed in inequality form, with pf^^ and e^mp given, 
the resulting optimization can be cast into SOCP form by 
introducing a new optimization variable 

X = (r,^-,^i, . . . ,^^, Ai, . . . , A^,z/) G 5ft^-+^+^ 

and defining appropriate selection matrices e.g., SqX = q, 
SjyX = jy, {S\x.)j = Xj, etc. We then have 

mm / X 



subject to 



||5gX-^Ve/|| < S^X 

^static'^q^ S ^static 

[A Azmp H~ b bzmp) J~o ^ 

\^zmp Pzrap^zmp) -J o\\ S ^zmp^zmp^ o 



(34) 

(35) 
(36) 

(37) 
(38) 



||P,,Ad^^.(5^x),-||<(5Ax),- VjG[l,m] (39) 

[Pmz - [pfmp] Pxy) ^o\\ < ^^X - C^^plixm^AX (40) 



-llxm^AX H SjyX < PzTo 



(41) 



^V^onsts^'i^-^^onsts \^^^,^ (42) 



To = i^SjrX 



(43) 



where To = M5^-x + C, H = [Ad^ • • • Ad^^] G 3?^><^^, 
/ = [1 Oix(2m+7V)]^ e 3?ix (2^+^+1), and 1 Ts a matrix all 
of whose elements are 1 . 

In the case of the problem addressed in [5], in which the 
COM constraints are given in equality form, it can be ex- 
pressed as a special case of our SOCP formulation by replacing 
(36) with (16), setting Czmp and pf^^ to zero, and eliminating 
Equations (38), (39), (43), and the variable A. Alternatively, 
one can express the ZMP constraints in equality form, and 
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the COM constraint in inequality form, to arrive at an SOCP 
formulation. Other objective functions subject to combinations 
of the above constraints (e.g., minimizing joint torques or joint 
accelerations subject to equality or inequality ZMP and COM 
constraints, and tracking the desired acceleration profile to 
within some prescribed bound) also admit a straightforward 
formulation as an SOCP problem; due to space limitations we 
do not provide details of these formulations here. 

C. Case Study 

Because of space limitations we present only one case study, 
involving kicking motions for a biped humanoid model. Ob- 
viously because of structural and inertial differences between 
the human and robot, the input motions need to be corrected 
in order for the robot to perform them without tipping over. It 
should also be apparent that some motions may be inherently 
unstabilizable because of, e.g., insufficient torque capacity, 
lack of friction, the motions may be excessively fast, etc. 





(a) Before stabilization 



(b) After stabilization 



Fig. 2. Case Study 1: ZMP trajectories for stabilized front kick. The dotted 
box represents the supporting polygon. 

The humanoid has 3 1 degrees of freedom and 25 actuated 
joints: six at each limb, and one at the rotational degree of 
freedom about the the waist. Values for the kinematic and 
inertial parameters are set to approximate a typical full-size 
biped humanoid, and the friction coefficient is set to 0.6. 
All simulations are performed in Matlab v6.5 running on 
a Pentium 1.4 GHz laptop computer. Each simulation time 
interval is 0.025 seconds. For the SOCP optimization, we use 
the Matlab toolbox provided by MOSEK. 

Computational experiments involving a front kick and side 
kick motion are performed for the most general SOCP op- 
timization as described in Equation (34); this optimization 
is repeatedly performed at each timestep over the entire 
duration of the motion. Setting Czmp to 0.5dmax for both 
motions, where dmax denotes the diameter of the smallest 
circle enclosing the support polygon, the computation time 
for both motions, which involves computing the COM and 
ZMP, calculating the dynamics, and performing the SOCP 
optimization, is 0.4 seconds. 

For the front kick motion, the ZMP trajectories before 
and after stabilization are shown in Figure 2. The small 
rectangle denotes the support polygon, and the x-axis denotes 
the forward direction along which the kick is directed. The 
figure clearly shows the ZMP always confined to the interior 
of the support polygon following our optimization procedure. 



For the above and other case studies (including those not 
reported here), and over a wide range of parameter values and 
scenarios, the SOCP optimization procedure shows remarkable 
consistency in terms of both computation times (around 0.4 
seconds, with a standard deviation of under 0.2 seconds) 
and the number of iterations per optimization (under 30). 
The friction coefficient, as expected, significantly impacts the 
active balancing algorithm. In cases where the friction coeffi- 
cient is less than 0.5, with complex input motions and large 
disturbances, solutions can fail to exist. That the algorithm 
is more effective with greater friction is not surprising, since 
balancing is far more difficult on slippery floors. 

Our motions are also affected by the choice of Czmp- As 
mentioned in our previous discussion on zero-moment point 
constraints, larger values of Czmp tend to magnify the effects 
of the extraneous rotational friction constraint. If Czmp is set 
to 0.5dmax (i-e., the diameter of the smallest circle enclosing 
the support polygon), and pf^^ is fixed to be the center 
of this circle, then any point inside the support polygon is 
a candidate ZMP (If Czmp = dmax^ then regardless of the 
choice of Pzmp ^^Y Point inside the support polygon is a 
candidate ZMP; values of Czmp larger than dmax are therefore 
superfluous). While choice of Czmp will to some extent require 
tuning for different platforms and application scenarios, in our 
case studies we have found Czmp < 0-^dmax effectively allows 
nearly all points within the support polygon to be a candidate 
ZMP 

Another choice affecting the stabilized motions is the actual 
support polygon S used in the optimization. Since during 
the course of the stabilization the projected COM approaches 
Ppcom ^ <S, 3. smaller S tends to lead to more stable motions. 
Also, the faster this convergence toward Ppcom (achieved by 
increasing the values for kd and kp, for example), the more 
quickly and efficiently the robot is able to react to dynamically 
changing environments and other disturbances. The obvious 
cost with a smaller S is that the solution space effectively 
becomes smaller as well. 



IV. Conclusion 

This paper has presented methods for generating optimal 
reaching and balancing movements in humanoid robots. In 
the case of reaching movements, we express movements as 
a linear combination of principal components extracted from 
arm motion data, and perform dynamic optimization using the 
gradient-based algorithm described in [1]. Active balancing 
is framed as a second-order cone programming problem, in a 
way such that existing approaches can be formulated as special 
cases within this SOCP framework. Our preliminary simu- 
lation results suggest that solutions are obtained efficiently 
and robustly. With increased computing power and modest 
enhancements in the algorithm, achieving online generation 
of these optimal movements appears to be a reachable goal. 
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